#include "ros/ros.h"
#include <fstream>
#include <iostream>
#include "nav_msgs/Odometry.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/TwistStamped.h"
#include "mavros_msgs/State.h"

using namespace std;

geometry_msgs::PoseStamped vicon_pose;
geometry_msgs::TwistStamped vicon_vel;
nav_msgs::Odometry setpoint;
mavros_msgs::State uav_state;

void mocapPosCallback(const geometry_msgs::PoseStamped &msg)
{
	vicon_pose = msg;
}

void mocapVelCallback(const geometry_msgs::TwistStamped &msg)
{
	vicon_vel = msg;
}

void setPointCallback(const nav_msgs::Odometry &msg)
{
	setpoint = msg;
}

void stateCallback(const mavros_msgs::State &msg)
{
	uav_state = msg;
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "msg2csv");

	ofstream outFile;
	outFile.open("data.csv", ios::out);

	ros::NodeHandle n;

	ros::Subscriber vicon_position_sub = n.subscribe("/mocap/pose", 1, mocapPosCallback);
	ros::Subscriber vicon_vel_sub = n.subscribe("/mocap/vel", 1, mocapVelCallback);
	ros::Subscriber setpoint_sub = n.subscribe("/ocplan_positiontwist", 1, setPointCallback);
	ros::Subscriber state_sub = n.subscribe("/mavros/state", 1, stateCallback);

	int LOOP_RATE = 20;
	ros::Rate loop_rate(LOOP_RATE);
	double t = 0.0;
	const double delt_t = 1.0 / LOOP_RATE;

	outFile << "t" <<"," << "px_sp" << "," << "py_sp" << "," << "pz_sp" << "," << "vx_sp" << "," << "vy_sp" << "," << "vz_sp" <<",";     
	outFile << "px" << "," << "py" << "," << "pz" << "," << "vx" << "," << "vy" << "," << "vz" << "\n";     
	int counter = 0;

	while(ros::ok())
	{
		if(uav_state.mode == "OFFBOARD"){
			ROS_INFO_THROTTLE(2, "Recording...");
			outFile << t <<",";
			outFile << setpoint.pose.pose.position.x << "," << setpoint.pose.pose.position.y << "," << setpoint.pose.pose.position.z << ",";
			outFile << setpoint.twist.twist.linear.x << "," << setpoint.twist.twist.linear.y << "," << setpoint.twist.twist.linear.z << ",";
			outFile << vicon_pose.pose.position.x << "," << vicon_pose.pose.position.y << "," << vicon_pose.pose.position.z << ",";
			outFile << vicon_vel.twist.linear.x << "," << vicon_vel.twist.linear.y << "," << vicon_vel.twist.linear.z << "\n";
			t += delt_t;
			counter = 0;
		}
		
		counter ++;
		if(counter > 100){
			counter = 101;
			ROS_INFO_THROTTLE(2, "No valid data to record");
		}

		ros::spinOnce();
		loop_rate.sleep();
	}

	outFile.close();

	return 0;
}